﻿from wbox.gx import GxProject
import io, array, struct
from ctypes import *
from wbox.gx import utils
from wbox.gx.program import GxCommand, GxStatement


class QCpu(object):
    def __init__(self):
        self.memory = QCpuMemory()
        self.ethernet = QEthernetModule(self)
        self.engine = QCpuEngine(self)

    def setup(self, path):
        self.project = GxProject(path)

    def run(self):
        self.engine.run(self.project)

        
class QCpuEngine(object):
    def __init__(self, cpu):
        self.cpu = cpu
        self._top = 0
        self._backup = 0
        self.stack = []
        
    def run(self, project):
        try:
            self.system()
            for prgName in project.programList():
                if prgName == "STEP":
                    continue
                program = project.program(prgName)
                for command in program:
                    if isinstance(command, GxCommand):
                        name = "_{0}_".format(command.name)
                        if hasattr(self, name):
                            getattr(self, name)(command)
                        else:
                            print "ignore command", command
                    elif isinstance(command, GxStatement):
                        pass
                    else:
                        print 'ignore', command
        except:
            print prgName, command
            raise
                
    def load(self, value):
        self._backup = self._top
        self._top = value
        
    def push(self, value):
        self.stack.append(value)
        
    def pop(self):
        return self.stack.pop()
        
    def top(self):
        return self.stack[-1]
        
    def system(self):
        self.setDevice([(0x91, 400)], 1) # SM400
        pass

    def getDevice(self, device, count=1):
        """
        post = device[-1][0]
        if post == 0xF0: # Z - index register
            pass
        elif post == 0xF2: # .
            return None
            
            
        """
        
        if len(device) == 1:
            code, address = device[0]
            if utils.isBitDevice(code):
                address, point = divmod(address, 16)
                values = self.cpu.memory.get(code, address, 1)[0]
                return 1 if (values & (1 << point)) != 0 else 0
            else:
                values = self.cpu.memory.get(code, address, count)
                if count == 1:
                    return values[0]
                return values
        print repr(list(reversed(device)))
        raise TypeError()
        
    def setDevice(self, device, value):
        if len(device) == 1:
            code, address = device[0]
            if utils.isBitDevice(code):
                address, point = divmod(address, 16)
                current = self.cpu.memory.get(code, address, 1)[0]
                current &= ~(1 << point)
                if value:
                    value = current | (1 << point)
                self.cpu.memory.set(code, address, [value])
            else:
                if type(value) == bool:
                    value = (0, 1)[value]
                if type(value) == int:
                    self.cpu.memory.set(code, address, [value])
                else:
                    self.cpu.memory.set(code, address, value)
            return
        raise TypeError()
        
    def getbit(self, device):
        return self.cpu.memory.getbit(device)
        
    def setbit(self, device, value):
        self.cpu.memory.setbit(device, value)

    def getint(self, device):
        return self.cpu.memory.getint(device)
        
    def setint(self, device, value):
        self.cpu.memory.setint(device, value)

    def getlong(self, device):
        return self.cpu.memory.getlong(device)
        
    def setlong(self, device, value):
        self.cpu.memory.setlong(device, value)

    def _LD_(self, command):
        device = command.arguments[0]
        self.load(self.getbit(device))
        
    def _LDI_(self, command):
        device = command.arguments[0]
        self.load(not self.getbit(device))
        
    def _LDP_(self, command):
        pass
        
    def _LDF_(self, command):
        pass
        
    def _LDEq_(self, command):
        pass
        
    def _LDNe_(self, command):
        pass
        
    def _LDLt_(self, command):
        pass
        
    def _LDLe_(self, command):
        pass
        
    def _LDGt_(self, command):
        pass
        
    def _LDGe_(self, command):
        pass
        
    def _LDDEq_(self, command):
        pass
        
    def _LDDNe_(self, command):
        pass
        
    def _LDDLt_(self, command):
        pass
        
    def _LDDLe_(self, command):
        pass
        
    def _LDDGt_(self, command):
        pass
        
    def _LDDGe_(self, command):
        pass
        
    def _ANDEq_(self, command):
        pass
        
    def _ANDNe_(self, command):
        pass
        
    def _ANDLt_(self, command):
        pass
        
    def _ANDLe_(self, command):
        pass
        
    def _ANDGt_(self, command):
        pass
        
    def _ANDGe_(self, command):
        pass
        
    def _ANDDEq_(self, command):
        pass
        
    def _ANDDNe_(self, command):
        pass
        
    def _ANDDLt_(self, command):
        pass
        
    def _ANDDLe_(self, command):
        pass
        
    def _ANDDGt_(self, command):
        pass
        
    def _ANDDGe_(self, command):
        pass
        
    def _OREq_(self, command):
        pass
        
    def _ORNe_(self, command):
        pass
        
    def _ORLt_(self, command):
        pass
        
    def _ORLe_(self, command):
        pass
        
    def _ORGt_(self, command):
        pass
        
    def _ORGe_(self, command):
        pass
        
    def _ORDEq_(self, command):
        pass
        
    def _ORDNe_(self, command):
        pass
        
    def _ORDLt_(self, command):
        pass
        
    def _ORDLe_(self, command):
        pass
        
    def _ORDGt_(self, command):
        pass
        
    def _ORDGe_(self, command):
        pass
        
    def _OR_(self, command):
        device = command.arguments[0]
        self._top = self._top or self.getbit(device)
        
    def _ORI_(self, command):
        device = command.arguments[0]
        self._top = self._top or not self.getbit(device)
        
    def _ORF_(self, command):
        pass
        
    def _ORP_(self, command):
        pass
        
    def _AND_(self, command):
        device = command.arguments[0]
        self._top = self._top and self.getbit(device)
        
    def _ANI_(self, command):
        device = command.arguments[0]
        self._top = self._top and not self.getbit(device)
        
    def _ANDP_(self, command):
        pass
        
    def _INV_(self, command):
        self._top = not self._top
        
    def _ORB_(self, command):
        self._top = self._backup or self._top
        
    def _ANB_(self, command):
        self._top = self._backup and self._top
        
    def _MPS_(self, command):
        self.push(self._top)
        
    def _MRD_(self, command):
        self._top = self.top()
        
    def _MPP_(self, command):
        self._top = self.pop()
        
    def _OUT_(self, command):
        device = command.arguments[0]
        self.setbit(device, self._top)
        
    def _SET_(self, command):
        device = command.arguments[0]
        self.setbit(device, 1)
        
    def _RST_(self, command):
        device = command.arguments[0]
        self.setbit(device, 0)
        
    def _FF_(self, command):
        pass
        
    def _MOV_(self, command):
        src  = command.arguments[0]
        dest = command.arguments[1]
        self.setint(dest, self.getint(src))
        
    def _DMOV_(self, command):
        src  = command.arguments[0]
        dest = command.arguments[1]
        self.setlong(dest, self.getlong(src))
        
    def _INC_(self, command):
        device = command.arguments[0]
        self.setint(device, self.getint(device) + 1)
        
    def _ADRSET_(self, command):
        src  = command.arguments[0]
        dest = command.arguments[1]
        addr = self.cpu.memory._getaddr(src)
        self.setlong(dest, addr)
        
    def _Add_(self, command):
        pass
        
    def _DAdd_(self, command):
        if len(command.arguments) == 3:
            src1 = command.arguments[0]
            src2 = command.arguments[1]
            dest = command.arguments[2]
        else:
            src1 = command.arguments[0]
            src2 = command.arguments[1]
            dest = command.arguments[1]
        self.setlong(dest, self.getlong(src1) + self.getlong(src2))
        
    def _Sub_(self, command):
        pass
        
    def _DSub_(self, command):
        src1 = command.arguments[0]
        src2 = command.arguments[1]
        dest = command.arguments[2]
        self.setlong(dest, self.getlong(src1) - self.getlong(src2))
        
    def _CALL_(self, command):
        pass
        
    def _FCALLP_(self, command):
        pass
        
    def _SUB_(self, command):
        pass
        
    def _RET_(self, command):
        pass

    def _BKRST_(self, command):
        pass

    def _BMOV_(self, command):
        pass
        
    def _FMOV_(self, command):
        pass
        
    def _FOR_(self, command):
        pass
        
    def _NEXT_(self, command):
        pass
        
    def _MC_(self, command):
        pass
        
    def _MCR_(elf, command):
        pass

    def _FEND_(self, command):
        pass
        
    def _END_(self, command):
        pass
        
def name2op(cls):
    import inspect
    ops = [("Add", "+"), ("Sub", "-"), ("Mul", "*"), ("Div", "/"),
        ("Eq", "="), ("Ne", "<>"), ("Lt", "<"), ("Le", "<="), ("Gt", ">"), ("Ge", ">=")]
    for name, member in inspect.getmembers(cls):
        for x, y in ops:
            if name.find(x) != -1:
                name = name.replace(x, y)
                member.im_func.func_name = name
                setattr(cls, name, member)
name2op(QCpuEngine)
del name2op


class QCpuMemory(object):
    _points_ = [
        (0x9C, 512),    # X  - Input relay
        (0x9D, 512),    # Y  - Output relay
        (0x90, 2048),   # M  - Internal relay
        (0x92, 2048),   # L  - Latch relay
        (0x93, 2048),   # F  - Annunciator
        (0x94, 2048),   # V  - Edge relay
        (0xA1, 2048),   # SB - Link Special
        (0xA0, 2048),   # B  - Link relay
        (0x91, 2048),   # SM - Special relay
        (0x98, 1024),   # S  - Step relay
        (0xC2, 69632),  # T  - Timer
        (0xC8, 69632),  # ST - Retentive timer
        (0xC5, 69632),  # C  - Counter
        (0xA8, 32768),  # D  - Data register
        (0xB4, 32768),  # W  - Link register
        (0xB5, 32768),  # SW - Link special
        (0xA9, 32768),  # SD - Special register
        (0xCC, 16),     # Z  - Index
        (0x9E, 5),      # FX - 
        (0x9F, 5),      # FY - 
        (0xAA, 5),      # FD - 
        (0xAF, 131072), # R  - File register
        (0x00, 2)]      # unknown
    #_total_points_ = reduce(lambda s, i: s + i[1], _points_, 0)

    def __init__(self):
        self.address = {}
        total = 0
        for point in self._points_:
            self.address[point[0]] = total
            total += point[1]
        for i in [0xC2, 0xC8, 0xC5]:
            self.address[i-1] = self.address[i]        # xS - Contact
            self.address[i-2] = self.address[i] + 2048 # xC - Coil
            self.address[i]   = self.address[i] + 4096 # xN - Current Value
        self.buffer = create_string_buffer(total * 2)
        self.memory = cast(self.buffer, POINTER(c_ushort))

    def __len__(self):
        return len(self.buffer)

    def __getitem__(self, index):
        if type(index) == tuple:
            index = self.address[index[0]] + index[1]
        return self.memory[index]

    def __setitem__(self, index, value):
        if type(index) == tuple:
            index = self.address[index[0]] + index[1]
        self.memory[index] = value

    def __getslice__(self, i, j):
        if type(i) == tuple:
            i = self.address[i[0]] + i[1]
        if type(j) == tuple:
            j = self.address[j[0]] + j[1]
        return self.memory[i:j]

    def __setslice__(self, i, j, value):
        if type(i) == tuple:
            i = self.address[i[0]] + i[1]
        if type(j) == tuple:
            j = self.address[j[0]] + j[1]
        for ii in range(j - i):
            self.memory[i + ii] = value[ii]

    def load(self, path):
        with open(path, "rb") as f:
            data = f.read()
            assert len(data) == len(self)
            memmove(self.buffer, data, len(data))

    def save(self, path):
        with open(path, "wb") as f:
            f.write(self.buffer.raw)

    def get(self, device, address, count):
        address = self.address[device] + address
        return self.memory[address:address+count]

    def set(self, device, address, values):
        address = self.address[device] + address
        values = array.array("H", values)
        for i in range(len(values)):
            self.memory[address + i] = values[i]
        
    def getbitaddr(self, device):
        dev, addr = device[-1]
        if utils.isBitDevice(dev) or utils.isTimerDevice(dev):
            if (len(device) > 1) and (device[-2][0] == 0xF0):
                addr += self[(0xCC, device[-2][1])]
            if utils.isTimerDevice(dev):
                dev -= 1
            addr, bit = divmod(addr, 16)
        elif device[-2][0] == 0xF2:
            bit = device[-2][1]
        else:
            raise TypeError("expected bit device, but {0} found".format(device))
        return self.address[dev] + addr, bit
        
    def getbit(self, device):
        addr, bit = self.getbitaddr(device)
        return (0, 1)[(self[addr] & (1 << bit)) != 0]
        
    def setbit(self, device, value):
        addr, bit = self.getbitaddr(device)
        self[addr] &= ~(1 << bit)
        if value:
            self[addr] |= (1 << bit)

    def getbits(self, device):
        addr, bit = self.getbitaddr(device)
        bits = device[0][1] * 4
        p = c_uint64.from_buffer(self.buffer, addr * 2)
        return (p.value >> bit) & ((1 << bits) - 1)
        
    def setbits(self, device, value):
        addr, bit = self.getbitaddr(device)
        p = c_uint64.from_buffer(self.buffer, addr * 2)
        bits = device[0][1] * 4
        mask = (1 << bits) - 1
        p.value = (p.value & ~(mask << bit)) | ((value & mask) << bit)
        
    def _getlong(self, (device, address)):
        address = self.address[device] + address
        return c_long.from_buffer(self.buffer, addr * 2).value
        
    def getaddr(self, device):
        dev, addr = 0, 0
        for _dev, _addr in reversed(device):
            if _dev == 0xF0: # Z - Index Qualification
                addr += self[(0xCC, _addr)]
            elif _dev == 0xF3: # @ - Indirect Designation
                return self._getlong((dev, addr)) 
            elif _dev in self.address: 
                dev, addr = _dev, _addr
            elif _dev == 0xAB: # G
                return self.address[0x00] # unknown
            else:
                raise TypeError("expected word device, but {0} found".format(device))
        return self.address[dev] + addr
        
    def getptr(self, device):
        dev, addr = 0, 0
        for _dev, _addr in reversed(device):
            if _dev == 0xF0: # Z - Index Qualification
                addr += self[(0xCC, _addr)]
            elif _dev == 0xF3: # @ - Indirect Designation
                return c_short.from_buffer(self.buffer, self._getlong((dev, addr)) * 2) 
            elif _dev in self.address: 
                dev, addr = _dev, _addr
            elif _dev == 0xAB: # G
                return self.address[0x00] # unknown
            else:
                raise TypeError("expected word device, but {0} found".format(device))
        return c_short.from_buffer(self.buffer, (self.address[dev] + addr) * 2)
        
    def getint(self, device):
        if device.isConstant():
            return device[-1][1]
        if device.isBitWord():
            return self.getbits(device)
        addr = self.getaddr(device)
        return self[addr]
        
    def setint(self, device, value):
        if device.isBitWord():
            return self.setbits(device, value)
        addr = self.getaddr(device)
        self[addr] = value
        
    def getlong(self, device):
        if device.isConstant():
            return device[-1][1]
        if device.isBitWord():
            return self.getbits(device)
        addr = self.getaddr(device)
        return c_long.from_buffer(self.buffer, addr * 2).value
        
    def setlong(self, device, value):
        if device.isBitWord():
            return self.setbits(device, value)
        addr = self.getaddr(device)
        c_long.from_buffer(self.buffer, addr * 2).value = value
        
    def _getaddr(self, device):
        if device.isBitDevice():
            return self.getbitaddr(device)[0]
        return self.getaddr(device)
        
        
class BinaryStream(io.BytesIO):
    def __init__(self, data=b""):
        super(BinaryStream, self).__init__(data)

    def read(self, n=None):
        data = super(BinaryStream, self).read(n)
        if not data:
            raise EOFError
        return data

    def readint1(self):
        return struct.unpack("b", self.read(1))[0]

    def readint2(self):
        return struct.unpack("H", self.read(2))[0]

    def readint4(self):
        return struct.unpack("l", self.read(4))[0]

    def readuint1(self):
        return struct.unpack("B", self.read(1))[0]

    def readuint2(self):
        return struct.unpack("H", self.read(2))[0]

    def readuint4(self):
        return struct.unpack("L", self.read(4))[0]

    def clear(self):
        self.truncate(0)

    def rewind(self):
        self.seek(0)


class QEthernetModule(object):
    def __init__(self, cpu):
        self.cpu = cpu

    def received(self, data):
        stream = BinaryStream(data)
        if stream.read(2) == b"\x50\x00":
            return self.receivedMcData(stream)
        return None

    def receivedMcData(self, stream):
        # Q header
        try:
            networkNo       = stream.readuint1()
            pcNo            = stream.readuint1()
            destModuleIO    = stream.readuint2()
            destStationNo   = stream.readuint1()
            dataLength      = stream.readuint2()
            monitoringTimer = stream.readuint2()

            command         = stream.readuint2()
            subcommand      = stream.readuint2()
            funcName = "_{0:04X}_{1:04X}".format(command, subcommand)
            if hasattr(self, funcName):
                return getattr(self, funcName)(stream)
        except EOFError:
            pass
        return None

    def _0401_0000(self, stream):
        device = stream.readuint4()
        count  = stream.readuint2()
        device, address = device >> 24, device & 0xffffff
        data = self.cpu.memory.get(device, address, count).tostring()
        print self.cpu

        result = BinaryStream()
        result.write("\xD0\x00")
        result.write("\x00")
        result.write("\xff")
        result.write("\xff\x03")
        result.write("\x00")
        result.write(struct.pack("H", len(data) + 2))
        result.write("\x00\x00")
        result.write(data)
        return result.getvalue()

    def _1401_0001(self, stream):
        device = stream.readuint4()
        count  = stream.readuint2()
        data   = stream.read(count // 2)

        result = BinaryStream()
        result.write("\xD0\x00")
        result.write("\x00")
        result.write("\xff")
        result.write("\xff\x03")
        result.write("\x00")
        result.write(struct.pack("H", 2))
        result.write("\x00\x00")
        return result.getvalue()

    def _1401_0000(self, stream):
        device = stream.readuint4()
        count  = stream.readuint2()
        data   = stream.read(count * 2)

        device, address = device >> 24, device & 0xffffff
        self.cpu.memory.set(device, address, data)

        result = BinaryStream()
        result.write("\xD0\x00")
        result.write("\x00")
        result.write("\xff")
        result.write("\xff\x03")
        result.write("\x00")
        result.write(struct.pack("H", 2))
        result.write("\x00\x00")
        return result.getvalue()


def main(path):
    cpu = QCpu()
    cpu.setup(path)
    cpu.run()

if __name__ == '__main__':
    main()
